/*
 * STMROS Dummy plugin
 */

#include "rcpputils/asserts.hpp"
#include "stmros/plugin.hpp"

namespace stmros
{
namespace std_plugins
{

/**
 * @brief Dummy plugin.
 */
class DummyPlugin : public plugin::Plugin
{
public:
  explicit DummyPlugin(StmRosPtr stmros_, const std::string & subnode = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Plugin(stmros_, subnode, options)
  {
    RCLCPP_INFO(get_logger(), "Dummy plugin initialized");
  }

  Subscriptions get_subscriptions() override
  {
    return {
      make_handler(0x01, &DummyPlugin::handle_heartbeat),
      make_handler(0x02, &DummyPlugin::handle_status)
    };
  }

private:
  void handle_heartbeat(const std::vector<uint8_t>& payload)
  {
    RCLCPP_INFO(get_logger(), "Dummy::handle_heartbeat: payload size %zu", payload.size());
    if (payload.size() >= 7) {
      RCLCPP_INFO(get_logger(), "Heartbeat version: %u", payload[6]);
    }
  }

  void handle_status(const std::vector<uint8_t>& payload)
  {
    RCLCPP_INFO(get_logger(), "Dummy::handle_status: payload size %zu", payload.size());
  }
};

} // namespace std_plugins
} // namespace stmros

#include "stmros/plugin_register_macro.hpp"
STMROS_PLUGIN_REGISTER(stmros::std_plugins::DummyPlugin)